#ifndef _RIGID_H
#define _RIGID_H

namespace math { namespace pbm {

#define GLFLOAT(VAL) VAL


template <typename value_type>
	matrix <value_type , 3, 3> normalize  (const matrix <value_type , 3, 3> & r ) 
{
	matrix <value_type , 3, 3> result ;
	result = quaterion2matrix ( matrix2quaterion (r).normalize() ) ;
	return result ;
}

template <typename value_type, unsigned dim > struct state  
{
public :
	typedef vector<value_type , dim > vector_type ;
	typedef vector_type point_type ;
	typedef matrix<value_type, dim , dim> matrix_type ;

	typedef HierarchyMesh<mesh_traits<value_type, 3> >   mesh_type ;
	typedef typename mesh_traits<value_type, 3>::FaceType      face_type;
	typedef state<value_type, dim> state_type ;
	typedef Polygon<value_type, dim>                polygon_type; 

	// State Variables 
	vector_type position ;
	matrix_type orientation ;
	vector_type linear_momentum ; 
	vector_type angular_momentum ; 

	// Derived quantities 
	vector_type velocity ;// v(t)
	vector_type omega ;   // w(t) 
	vector_type force  ;  // f (t)
	vector_type torque ;  // \tou(t)


	// Auxiliary vairables
	matrix_type Iinv    ; // I^(-1) (t)
	vector_type mass_center ;

	// model 
	mesh_type model ; 

	value_type m_time ; // Current time 
	bool m_static ; 
   
	std::vector<state_type> * mv_objects ;
public :
	
	void init_variables ()
	{
		m_mass = 1.0f ; // compute the mass 
		orientation = 1, 0, 0, 0, 1, 0, 0, 0, 1; 

		// Ibody, Ibodyinv ;
		const value_type x = 2.0 ;
		const value_type y = 1.5 ;
		const value_type z = 1.0 ; 

		Ibody = y* y + z * z, 0, 0,
			0, x * x + z * z, 0  , 
			0, 0, x * x + y * y ; 
		Ibody *= m_mass / 12.0f; 

		Ibodyinv = 1.0 / Ibody[0][0],  0, 0 ,
			0,  1.0 / Ibody[1][1], 0, 
			0, 0, 1.0 / Ibody[2][2] ;

		std::cout<< Ibody<<std::endl;
		std::cout<< Ibodyinv<<std::endl;
		// Omega 
		//omega = 0.0, 0.0, 0.001; 

	}
public :
	//update state to time t
	void back (value_type & t)
	{
	}
	void advent (value_type & t)
	{

		velocity = compute_velocity(); 
		omega = compute_omega (); 
		force = compute_force (); 
		torque = compute_torque ();

		vector_type old_position = position ;
		matrix_type old_orientation = orientation ;
		vector_type old_linear_momentum = linear_momentum ;
		vector_type old_angular_momentum = angular_momentum ;

		point_type cpoint ;
		vector_type cnormal ;
		value_type dis ;
		state_type * pobject ; // create object here. FIX it. 

		position += velocity * t ;
		orientation += star (omega) * orientation * t ;
		linear_momentum += force *  t ;
		angular_momentum +=  torque * t ;

		if (collide_detect (pobject, cpoint, cnormal, dis)) 
		{

			// evaluate the correct collide time 
			point_type local_point = trans_global2local (cpoint);
			point_type old_cpoint = trans_local2global (local_point, old_orientation, old_position ); 

			std::cout<<"OLD CPOINT "<<old_cpoint<<' '<<cpoint<<local_point<<std::endl;
			// back the state 
			/*
			position += velocity * (-back_t) ;
			orientation += star (omega) * orientation * (-back_t) ;
			linear_momentum += force *  (-back_t) ;
			angular_momentum += torque * (-back_t) ;
			*/
			// state has been changed .
			//collide_response (pobject, cpoint, cnormal, dis) ;  // ground is the b object 
		}	




	
	}
	void integration (value_type time) 
	{
		for ( ; time > 0; time -= m_time_step)
		{
			value_type t = m_time_step ;
			advent (t) ;
		}
		time += m_time_step ;
		advent (time ); 
/*
		std::cout<<"*********************"<<std::endl;
		std::cout<<"X "<<position<<std::endl;
		std::cout<<"V "<<velocity<<std::endl;
		std::cout<<"P "<<linear_momentum <<std::endl;
		std::cout<<"L "<<angular_momentum<<std::endl;
		std::cout<<"*********************"<<std::endl;
*/
	}
	void update (value_type time)
	{
		if (true == this->m_static ) return ; // keep static object
		value_type t = time - m_time ;
		m_time = time ;
		integration (t);
	}
protected :

	vector_type trans_global2local (const vector_type & coord, const matrix_type & r, const vector_type & trans) const 
	{
		return (r.inv()) * (coord - trans) ; // r.transpose cannot be used to compute the inverse
	} 
	vector_type trans_global2local (const vector_type & coord) const 
	{
		return trans_global2local(coord, this->orientation, this->position) ;
		//return orientation.transpose () * ( coord - position ) ;
	}

	vector_type trans_local2global (const vector_type & coord, const matrix_type & r, const vector_type & trans) const
	{
		return (r * coord + trans );
	} 
	vector_type trans_local2global (const vector_type & coord) const 
	{
		return trans_local2global (coord, this->orientation, this->position);
//		return orientation *  coord  + position ;
	}
	polygon_type trans_local2global (const polygon_type & pol) const 
	{
		polygon_type result ;
		for (unsigned i = 0; i < pol.size(); ++i)
		{
			result.append_vertex (trans_local2global (pol[i]));
		}
		return result ;
	}
	polygon_type trans_global2local (const polygon_type & pol) const 
	{
		polygon_type result ;
		for (unsigned i = 0; i < pol.size(); ++i)
		{
			result.append_vertex (trans_global2local(pol[i]));
		}
		return result ;
	}
	polygon_type face2polygon (const face_type * face) const 
	{
		polygon_type pol ;
		std::vector<point_type *> v_set ;
		face->Face2VertexCoord (v_set);
		for (unsigned i = 0; i  < v_set.size(); ++i)
			pol.append_vertex ( * v_set[i]);
		return pol ;
	}


	value_type evaluate_dis (const polygon_type & pol, const point_type & cpoint, const vector_type & cnormal) const 
	{
		return (cpoint - pol[0]) * cnormal ; 
		//return (cpoint.z + 4.0) ;
	}
	//v_face_id return the intersected face id. 
	bool intersect (const state_type & test_obj, point_type & cpoint, vector_type & cnormal, value_type & dis) const
	{
		// Note. This intersect is not fully correct.  FIX ME HERE
		// We have to classify the face-vertex and edge edge collision
		// The face-vertex and vertex-face are different


		// In this implementation we traverse the plan. 
		// This should be simplifed. 
		for (unsigned i = 0; i < test_obj.model.faces.size(); ++i)
		{
			polygon_type pol; 
			pol = face2polygon (test_obj.model.faces[i]) ;
			pol = test_obj.trans_local2global (pol);
			// compute the collide normal
			cnormal = (pol[1] - pol[0]) ^ (pol[2] - pol[0]);
			cnormal.normalize();
			polygon_type global_pol = pol ;
			pol = this->trans_global2local (pol);				
			std::vector<unsigned> v_face_id ;
			if (this->model.intersect (pol, v_face_id) ) 
			{
				std::cout<<"CNORMAL "<<cnormal <<std::endl;
				polygon_type self_face = face2polygon(this->model.faces[v_face_id[0]]) ;// pick the first face


				// do orientation test
				for (unsigned j = 0; j < 3 ; ++j)
				{
					/*
					cpoint = this->trans_local2global(self_face[j]); 
					
					if ( cpoint.z < -4.0) 
					{
						cnormal = vector_type (0, 0, 1); 
						std::cout<<"CNORMAL "<<cnormal <<std::endl;
						std::cout<<"CPOINT "<<cpoint<<" ID "<<std::endl;
						dis = cpoint.z + 4.0; 
						std::cout<<"DIS "<<dis<<std::endl;
						return true; 
					}
					*/
					
					if (orient_3d (self_face[j], pol[0], pol[1], pol[2]) >= 0)
					{
						cpoint = this->trans_local2global(self_face[j]);
						std::cout<<"CPOINT "<<cpoint<<" ID "<<std::endl;
						dis = evaluate_dis (global_pol, cpoint, cnormal) ;
						std::cout<<"DIS "<<dis<<std::endl;
						return true ;
					}
					
					
				}
				throw ("Fail to find the point");
				return true; 
			}
		}
		return false ;
	}

	bool collide_detect (state_type * & pobj, point_type & cpoint, vector_type & cnormal, value_type & dis) 
	{
		// normalize the matrix
		orientation = normalize(orientation); 
		state_type & test_obj = (*mv_objects)[mv_objects->size() - 1] ; // the last model ;
		pobj = & test_obj ;

		/*
		for (unsigned i = 0; i < this->model.vertices.size(); ++i)
		{
			cpoint = trans_local2global(model.vertices[i]->m_coord) ;
			if ( cpoint.z < -4.0) 
			{
//				std::cout<<"CPOINT LOCAL "<<model.vertices[i]->m_coord<<std::endl;
//				std::cout<<"REVERSE MAPP "<<this->trans_global2local (cpoint)<<std::endl; 
				cnormal = vector_type (0, 0, 1); 
				std::cout<<"CNORMAL "<<cnormal <<std::endl;
				std::cout<<"CPOINT "<<cpoint<<" ID "<<std::endl;
				dis = cpoint.z + 4.0; 
				std::cout<<"DIS "<<dis<<std::endl;
				return true; 
			}
		}
		return false; 
		*/
		/*
		if (this->intersect (test_obj, cpoint, cnormal, dis)) 
		{
			pobj = & test_obj ;
			collide_response (pobj, cpoint, cnormal, dis) ;  // ground is the b object 
			return true; 
		}
		return false ;
		*/
		
		
		for (unsigned i = 0; i < mv_objects->size(); ++i)
		{
			pobj = & ((*mv_objects)[i]) ;
			if ( pobj ==  this) continue ;// test with the object self. 

			state_type & test_obj = (*mv_objects) [i] ;
			if (this->intersect (test_obj, cpoint, cnormal, dis)) 
			{
				std::cout<<"Collide POINT " <<cpoint<<std::endl;
				std::cout<<"Collide Normal " <<cnormal<<std::endl;
				collide_response (pobj, cpoint, cnormal, dis) ;  // ground is the b object 
				//collide_response ((*mv_objects)[mv_objects->size() - 1], cpoint, cnormal, dis) ;  // ground is the b object 
				return true; 
			}
		}
		

		return false; 
	}
	void collide_response (state_type * pb, point_type & cpoint, vector_type & cnormal, value_type & dis) 
	{
		std::cout<<"collide response "<<std::endl; 
		/**
		   1. Find collision point
		   2. Write point as  ra, rb 
		   3. Calculate pa-, pb-
		   4. calculate vrel-
		   5. calculate j
		   6. update pa, la , pb, lb 
		 */
		state_type & b = * pb; 
		state_type & a = * this;  // we have a & b object. 
		std::cout <<a.model.vertices.size()<<std::endl;
		std::cout <<b.model.vertices.size()<<std::endl;
		vector_type & point = cpoint ;//trans_local2global (a.model.vertices[idx]->m_coord) ;
		//value_type dis = point.z + 4.0;  
		//value_type dis = -0.2; 
		std::cout<<"DIS "<<dis<<std::endl;
		std::cout<<"POINT "<<point <<std::endl;
		vector_type ra = point - position ; // trans_local2global ( a.mass_center);
		vector_type rb = point - b.position ;
		std::cout<<"RA1 "<<ra<<std::endl;
		std::cout<<"VA "<<a.velocity<<' '<<a.omega<<' '<<ra<<std::endl;
		vector_type dotpa = a.velocity + (a.omega ^ ra)  ;
		vector_type dotpb = b.velocity + (b.omega ^ rb)  ;

		std::cout<<"DOTPA "<<dotpa<<std::endl;
		std::cout<<"DOTPB "<<dotpb<<std::endl;

		vector_type & normal = cnormal ;
		value_type pre_v = normal *  (dotpa - dotpb) ;
		value_type numerator = - (1.0 + epsi) * pre_v ;

		value_type term1 = 1.0 / a.m_mass  ;
		value_type term2 = 1.0 / b.m_mass  ;
		value_type term3 = normal * (a.Iinv * (ra ^ normal ) ^ ra) ;
		value_type term4 = normal * (b.Iinv * (rb ^ normal ) ^ rb) ;

		value_type j = numerator / (term1 + term2 + term3 + term4) ; 
		vector_type force = normal * j ;
		std::cout<<"FORCE "<<force <<std::endl;

		a.linear_momentum += force ;
		b.linear_momentum -= force ;
		a.angular_momentum += (ra ^ force) ;
		b.angular_momentum -= (rb ^ force) ;
		
		a.position += (-dis + 1e-7) * cnormal;
	}
	// compute the derivative begin
	vector_type compute_velocity () 
	{
		return this->linear_momentum / m_mass ;
	}

	vector_type compute_omega () 
	{
		this->Iinv = this->orientation * this->Ibodyinv * (this->orientation.transpose()); 
		this->omega = this->Iinv * this->angular_momentum ;
		return this->omega ;
	}

	vector_type compute_force () 
	{
		return vector_type (0, 0, -1.0);  
	}
	vector_type compute_torque () 
	{
		return vector_type (0.0, 0, 0.0)  ;
	}
	// compute the derivative end 
	

private : 
	value_type m_mass ; // mass
	matrix_type Ibody ; // Ibody 
	matrix_type Ibodyinv ; // Inverse of Ibody 
	static constexpr value_type m_time_step = 0.01 ; // for integration. 
	static constexpr value_type epsi  = 0.95 ; // bouncing coefficient. 0 \leq epsi \leq 1.
	static constexpr value_type g = 9.8 ;      // gravity coefficient. 
};


template <typename value_type, unsigned dim> struct system 
{
	typedef vector<value_type , dim > vector_type ;
	typedef matrix<value_type, dim , dim> matrix_type ;
	typedef state <value_type, dim> state_type ;
	typedef HierarchyMesh<mesh_traits<value_type, 3> >   mesh_type ;

	typedef math::vector<value_type, 3>                          point3 ;
	typedef math::vector<value_type, 2>                          point2 ;
	typedef math::vector<value_type, 3>                          vector3 ;
	typedef math::vector<value_type, 2>                          vector2 ;

public :
	system (unsigned size) : m_sys_time(0), m_num_body(size) 
	{
		// allocate memory 
//		this->mv_model.resize (m_num_body ) ; 
		this->mv_state.resize (m_num_body ) ; 
	}

	void init_model () 
	{
		std::string filename ;

		for (unsigned i = 0; i < m_num_body ; ++i)
		{
			this->mv_state[i].init_variables(); 
		}

		// load the block
		std::cin>>filename ;
		for (unsigned i = 0; i < m_num_body - 1; ++i)
		{
			std::ifstream ifs1 (filename.c_str()) ;
			std::cout<<filename<<std::endl; 
			ifs1 >> mv_state[i].model ; 
			mv_state[i].model.scale(0.5);
			//mv_state[i].model.rotateX(0.5); 
			//mv_state[i].model.rotateY(0.5); 
			MatrixRotateX (mv_state[i].orientation, 0.5) ;
			MatrixRotateY (mv_state[i].orientation, 0.5); 
			//mv_state[i].position = vector_type (i * 2.0 , 0 , 3.0);
			//mv_state[i].mass_center = vector_type ( i * 2.0, 0, 3.0) ;
			mv_state[i].position = vector_type (0, 0, i * 2.0 + 1.0); 
			mv_state[i].mass_center = vector_type (.0, 0, i * 2.0 + 1.0); 
			mv_state[i].m_static = false ;
			mv_state[i].mv_objects = & mv_state ;
		}

		// load the plane
		unsigned idx = m_num_body - 1;
		std::cin>>filename ;
		std::ifstream ifs2 (filename.c_str());
		std::cout<<filename<<std::endl; 
		ifs2 >> mv_state[idx].model ;
		//mv_state[2].model.translate(vector_type (0.0, 0.0, -4.0) );
		mv_state[idx].mass_center = vector_type (0.0, 0.0, -4.0) ;
		mv_state[idx].position = vector_type (0.0, 0.0, -4.0) ; 
		mv_state[idx].m_static = true ;  // static object 
		mv_state[idx].mv_objects = & mv_state ;


		// build_hierarchy 
		for (unsigned i = 0; i < m_num_body ; ++i)
		{
			mv_state[i].model.build_hierarchy(); 
		}
	}

	void update () 
	{
		m_sys_time += m_frame_latency ;
		for (unsigned i = 0; i < this->m_num_body ; ++i)
		{
//			std::cout<<"Update Object "<<i<<std::endl; 
			mv_state[i].update (m_sys_time); 
		}
	} 
public :
	void draw_models()
	{

		for (unsigned i = 0; i < this->m_num_body ; ++i)
		{
			if (i < this->m_num_body - 1)
			{
				GLfloat material_Ka[] = {.5f, .0f, .0f, 1.0f};
				glMaterialfv(GL_FRONT_AND_BACK, GL_AMBIENT, material_Ka);
			}
			else 
			{
				GLfloat material_Ka[] = {.2f, .2f, .2f, 1.0f};
				glMaterialfv(GL_FRONT_AND_BACK, GL_AMBIENT, material_Ka);
		
			}
			draw_mesh_facet (& mv_state[i].model , & mv_state[i] ); 
		}
	}
	unsigned get_size () {return this->m_num_body ;}
protected :
protected :
	void draw_triangles_facet (const std::vector<point3 *> & point, const vector3 & normal, int faceid	,	std::vector<int> pv, state_type * state)
	{

		glBegin(GL_POLYGON);
		glNormal3f(GLFLOAT(normal[0]), GLFLOAT(normal[1]), GLFLOAT(normal[2])); 

		for (unsigned i = 0; i < point.size(); ++i)
		{
			point3 p ; 
			p = state->orientation * (*point[i]) + state->position ;
			glVertex3f (GLFLOAT (p.x), GLFLOAT (p.y), GLFLOAT (p.z));
//			glVertex3f( GLFLOAT((*point[i])[0]), GLFLOAT((*point[i])[1]), GLFLOAT((*point[i])[2]));
		}
		glEnd();
	}

	void draw_mesh_facet (mesh_type * mesh, state_type * state)
	{
		for (unsigned i = 0; i < mesh->faces.size(); ++i)
		{
			std::vector<point3 *> p;
			mesh->faces[i]->Face2VertexCoord(p); 
			std::vector<int> pv;
			mesh->faces[i]->Face2VertexIdx(pv);
			draw_triangles_facet ( p, mesh->faces[i]->normal(), i, pv, state); 
		}	
	}
	// init state 
	
private :
	std::vector <state_type> mv_state ;
private :
	unsigned m_num_body ;
	state_type m_state; 
	value_type m_sys_time ;
	static constexpr value_type m_num_frame = 24.0 ;
	static constexpr value_type m_frame_latency = 1.0 / m_num_frame ;

};

class RigidFrame : public math::ui::GLFrame
{
public :
	typedef double value_type ;

	typedef vector<value_type , 3> vector_type ;
	typedef matrix<value_type, 3, 3> matrix_type ;
	typedef state <value_type, 3> state_type ;
	typedef HierarchyMesh<mesh_traits<value_type, 3> >   mesh_type ;

public :
	RigidFrame () : GLFrame () , light(false ), pause (false )
	{
		mp_system = new system <double, 3> (2); // two models 
	}
	~ RigidFrame ()  
	{
		delete mp_system; 
	}
	void init_light(void)
	{
		glEnable(GL_LIGHTING);
		glEnable(GL_LIGHT0);
		glEnable(GL_NORMALIZE);

		// Light model parameters:
		// -------------------------------------------

		GLfloat lmKa[] = {0.0, 0.0, 0.0, 0.0 };
		glLightModelfv(GL_LIGHT_MODEL_AMBIENT, lmKa);

		glLightModelf(GL_LIGHT_MODEL_LOCAL_VIEWER, 1.0);
		glLightModelf(GL_LIGHT_MODEL_TWO_SIDE, 0.0);

		// -------------------------------------------
		// Spotlight Attenuation

		GLfloat spot_direction[] = {1.0, -1.0, -1.0 };
		GLint spot_exponent = 30;
		GLint spot_cutoff = 180;

		glLightfv(GL_LIGHT0, GL_SPOT_DIRECTION, spot_direction);
		glLighti(GL_LIGHT0, GL_SPOT_EXPONENT, spot_exponent);
		glLighti(GL_LIGHT0, GL_SPOT_CUTOFF, spot_cutoff);

		GLfloat Kc = 1.0;
		GLfloat Kl = 0.0;
		GLfloat Kq = 0.0;

		glLightf(GL_LIGHT0, GL_CONSTANT_ATTENUATION,Kc);
		glLightf(GL_LIGHT0, GL_LINEAR_ATTENUATION, Kl);
		glLightf(GL_LIGHT0, GL_QUADRATIC_ATTENUATION, Kq);


		// ------------------------------------------- 
		// Lighting parameters:

		GLfloat light_pos[] = {0.0f, 5.0f, 5.0f, 1.0f};
                
		GLfloat light_Ka[]  = {0.8f, .8f, .8f, 1.0f};
		GLfloat light_Kd[]  = {.5f, .3f, 0.9f, 1.0f};
		GLfloat light_Ks[]  = {.8f, .8f, .8f, 1.0f};

		//GLfloat light_Ka[]  = {1.0f, 0.5f, 0.5f, 1.0f};
		//GLfloat light_Kd[]  = {1.f, 0.1f, 0.1f, 1.0f};
		//GLfloat light_Ks[]  = {1.0f, 1.0f, 1.0f, 1.0f};

		glLightfv(GL_LIGHT0, GL_POSITION, light_pos);
		glLightfv(GL_LIGHT0, GL_AMBIENT, light_Ka);
		glLightfv(GL_LIGHT0, GL_DIFFUSE, light_Kd);
		glLightfv(GL_LIGHT0, GL_SPECULAR, light_Ks);

		// -------------------------------------------
		// Material parameters:

		//GLfloat material_Ka[] = {0.5f, 0.0f, 0.0f, 1.0f};
		//GLfloat material_Kd[] = {0.4f, 0.4f, 0.5f, 1.0f};
		//GLfloat material_Ks[] = {0.8f, 0.8f, 0.0f, 1.0f};
		GLfloat material_Ka[] = {.5f, .5f, .5f, 1.0f};
		GLfloat material_Kd[] = {0.4f, 0.4f, 0.4f, 1.0f};
		GLfloat material_Ks[] = {0.8f, 0.8f, 0.8f, 1.0f};
		GLfloat material_Ke[] = {0.1f, 0.1f, 0.1f, 0.0f};
		GLfloat material_Se = 20.0f;

		glMaterialfv(GL_FRONT_AND_BACK, GL_AMBIENT, material_Ka);
		glMaterialfv(GL_FRONT_AND_BACK, GL_DIFFUSE, material_Kd);
		glMaterialfv(GL_FRONT_AND_BACK, GL_SPECULAR, material_Ks);
		glMaterialfv(GL_FRONT_AND_BACK, GL_EMISSION, material_Ke);
		glMaterialf(GL_FRONT_AND_BACK, GL_SHININESS, material_Se);

	}

	virtual void initGL () 
	{
		GLFrame::initGL (); 
		init_light();
		glDisable(GL_LIGHTING);
		glPolygonMode(GL_FRONT_AND_BACK, GL_LINE);
		glEnable(GL_DEPTH_TEST);
		glEnable(GL_CULL_FACE);
/*
		glEnable(GL_DEPTH_TEST);
		glDisable(GL_LIGHTING);
*/
		// load models; 
		mp_system->init_model (); 
	}
	virtual void keyboard (unsigned char key, int x, int y)
	{
		switch (key)
		{
		case 'l' : case 'L' : light = !light ; break; 
		case 'p' : case 'P' : pause = !pause ; break; 
		}
		glutPostRedisplay();
	}
	virtual void sence () 
	{ 
		//glutSolidSphere(0.5, 40, 40) ;
		if (light) 
		{
			glPolygonMode(GL_FRONT_AND_BACK, GL_FILL);
			glEnable(GL_LIGHTING);

		}
		else 
		{
			glPolygonMode(GL_FRONT_AND_BACK, GL_LINE);
			glDisable(GL_LIGHTING);
		}
		glRotatef (-90, 1.0, 0.0, 0.0); 
		mp_system->draw_models(); 
	}

	virtual void idle () 
	{
		if (pause == false)
			mp_system->update (); 

		GLFrame::idle (); 
	}

protected :
	system <double, 3> * mp_system ; 
	bool light ;
	bool pause ;
};

}}

#endif
